#include "FlyWheels.h"
#include "../Robotmap.h"

FlyWheels::FlyWheels() : Subsystem("FlyWheels") {
	
	flyWheelMotor_1 = new Jaguar(RobotMap::OTHER_FLYWHEEL_MOTOR_1);
	flyWheelMotor_2 = new Jaguar(RobotMap::OTHER_FLYWHEEL_MOTOR_2);
	flyWheelsOn = false;
}

void FlyWheels::InitDefaultCommand() {
}

float FlyWheels::getSpeedOfMotorOne() {
	return flyWheelMotor_1->Get();
}


float FlyWheels::getSpeedOfMotorTwo() {
	return flyWheelMotor_2->Get();
}

void FlyWheels::setFullSPeed() {
	flyWheelMotor_1->SetSpeed(1.0);
	flyWheelMotor_2->SetSpeed(1.0);
	flyWheelsOn = true;
}

void FlyWheels::setHalfSpeed() {
	flyWheelMotor_1->SetSpeed(0.5);
	flyWheelMotor_2->SetSpeed(0.5);
	flyWheelsOn = true;
}

void FlyWheels::stopFlyWheel() {
	flyWheelMotor_1->SetSpeed(0.0);
	flyWheelMotor_2->SetSpeed(0.0);
	flyWheelsOn = false;
}

void FlyWheels::setLowerMotorSpeed(float v) {
	flyWheelMotor_1->SetSpeed(v);
}

void FlyWheels::setUpperMotorSpeed(float v) {
	flyWheelMotor_2->SetSpeed(v);
}

void FlyWheels::setFlyWheelsOn(bool b) {
	flyWheelsOn = b;
}

void FlyWheels::setFlyWheelSpeed(float lower, float upper) {
	setLowerMotorSpeed(-lower);
	setUpperMotorSpeed(upper);
	
	flyWheelsOn = true;	
}

bool FlyWheels::isFlyWheelOn() {
	return flyWheelsOn;
}
